#!/usr/bin/python

# 计算步进电机在恒定加速度下加速运动时每一步所需的速度
# 以便于实现梯形加速运动控制

import numpy as np
import matplotlib.pyplot as plt

# 最大速度(Hz)
Vmax = 10000

# 启动速度，即最小速度(Hz)
Vmin = 200

# 从静止加速到最大速度所走的路程(步)
Sacc = 200

# 速度-路程关系
def vs(s, acc=1):
    return Vmin + (acc * s) ** 0.5

# 路程-速度关系
def sv(v, acc=1):
    return ((v - Vmin)**2) / acc

def hz2ns(hz):
    return 1_000_000_000 / hz

# 加速度(Hz/s)
ACC = ((Vmax - Vmin)**2) / Sacc
print(f"Sacc: {Sacc}")
print(f"ACC: {ACC}")

s = np.linspace(0, Sacc, Sacc)
v = vs(s, ACC)
print(f"v:\n{v}")
print(f"ns:\n{hz2ns(v)}")

plt.plot(s, v, label=f"acc={ACC}")
plt.legend()
plt.show()
